/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-12-06     MBW       the first version
 */
#include "pid2.h"
#include "maths.h"
#include "PPM.h"
#include "motor.h"
#include "data_define.h"
#include "sys.h"
#include "JY901B_931.h"
#include "delay_uil.h"
#define SateControl
#define HeightControl
#define OpenYawControl

#define OpenFlowControl
#define positioinControl

//#define SetThrustRate

void control_task(void *p);
void control_task_init(void)
{
    rt_thread_t tid1 = RT_NULL , tid2 = RT_NULL;
    //**********参数初始化**********
    control_data.limit_vel_up.z   = 30;
    control_data.limit_vel_down.z = -5;
    control_data.limit_vel_up.x   = 25; control_data.limit_vel_down.x = -25;
    control_data.limit_vel_up.y   = 25; control_data.limit_vel_down.y = -25;
    control_data.A = 660;
    control_data.A_H = 110;
    control_data.T = 5;
    control_data.W = 3.1415/(2*control_data.T);
    TIM4->CCR1 = 1000; //设置第一通道PC6
    TIM4->CCR2 = 1000; //设置第二通道PC7
    TIM4->CCR3 = 1000; //设置第三通道PC8
    TIM4->CCR4 = 1000; //设置第四通道PC69
    //x=>pitch轴角度和角速度PID控制器参数
    PID_PAR.angel_kp.x = 2 ; PID_PAR.angel_ki.x = 0.05 ; PID_PAR.angel_kd.x = 0; //roll 2.6 0.05 0
    PID_PAR.rate_kp.x  = 1 ; PID_PAR.rate_ki.x = 0.0974 ; PID_PAR.rate_kd.x = 10; //roll 1.2 0.0974 12
    //y=>roll轴角度和角速度PID控制器参数
    PID_PAR.angel_kp.y = 2 ; PID_PAR.angel_ki.y = 0.05 ; PID_PAR.angel_kd.y = 0; //roll
    PID_PAR.rate_kp.y  = 0.2 ; PID_PAR.rate_ki.y = 0.0974 ; PID_PAR.rate_kd.y = 0; //roll
    //z=>yaw轴角度和角速度PID控制器参数
    PID_PAR.angel_kp.z = 2 ; PID_PAR.angel_ki.z = 0.05 ; PID_PAR.angel_kd.z = 0; //yaw
    PID_PAR.rate_kp.z = 1.8 ; PID_PAR.rate_ki.z = 0.0274 ; PID_PAR.rate_kd.z = 0; //yaw
    //v=>三轴速度PID控制器参数
    PID_PAR.vel_kp.x = 0.2 ; PID_PAR.vel_ki.x = 0.00274 ; PID_PAR.vel_kd.x = 0;
    PID_PAR.vel_kp.y = 0.2 ; PID_PAR.vel_ki.y = 0.00274 ; PID_PAR.vel_kd.y = 0;
//    PID_PAR.xvp = 0.2 ; PID_PAR.xvi = 0 ; PID_PAR.xvd = 0;
//    PID_PAR.yvp = 0.2 ; PID_PAR.yvi = 0 ; PID_PAR.yvd = 0;
    PID_PAR.vel_kp.z = 1.0;      PID_PAR.vel_ki.z =0.09;    PID_PAR.vel_kd.z = 0;  //1.5  0.09  0
    //位置控制器PID参数
    PID_PAR.dis_kp.x = 1.0 ,PID_PAR.dis_ki.x = 0,PID_PAR.dis_kd.x=0;
    PID_PAR.dis_kp.y = 1.0 ,PID_PAR.dis_ki.y = 0,PID_PAR.dis_kd.y=0;
    PID_PAR.dis_kp.z = 1.8 ;PID_PAR.dis_ki.z = 0.01;PID_PAR.dis_kd.z = 0 ;               //2 0.01 0
    //**********任务初始化**********

    tid1 = rt_thread_create("control_task",control_task, RT_NULL, 2000, con_task_prio, 20);
    if (tid1 != RT_NULL)rt_thread_startup(tid1),rt_kprintf("...control_task is ok!\n");
}
INIT_APP_EXPORT(control_task_init);
float height_last_out = 0,cas_PID_out =0 ,out_disPID=0;
void pid_control(void)
{
#ifdef positioinControl

    control_data.set_vel.x = vel_pid_x_test(control_data.set_position.x ,control_data.get_position.x
                                            ,PID_PAR.dis_kp.x,PID_PAR.dis_ki.x,PID_PAR.dis_kd.x);

    control_data.set_vel.y = vel_pid_y_test(control_data.set_position.y ,control_data.get_position.y
                                            ,PID_PAR.dis_kp.y,PID_PAR.dis_ki.y,PID_PAR.dis_kd.y);

    control_data.set_vel.x = constrainf(control_data.set_vel.x,control_data.limit_vel_down.x,control_data.limit_vel_up.x);
    control_data.set_vel.y = constrainf(control_data.set_vel.y,control_data.limit_vel_down.y,control_data.limit_vel_up.y);
#endif
#ifdef OpenFlowControl
    control_data.set_angel.x = vel_pid_x_test(control_data.set_vel.x ,control_data.get_vel.x
                                ,PID_PAR.vel_kp.x,PID_PAR.vel_ki.x,PID_PAR.vel_kd.x);

    control_data.set_angel.y = vel_pid_y_test(control_data.set_vel.y ,control_data.get_vel.y
                                ,PID_PAR.vel_kp.x,PID_PAR.vel_ki.x,PID_PAR.vel_kd.x);
#endif

#ifdef SateControl
    control_data.pitch = cascade_pid_pitch_test(control_data.set_angel.x ,control_data.get_angel.x ,control_data.get_gyro.y
                                ,PID_PAR.angel_kp.x,PID_PAR.angel_ki.x,PID_PAR.angel_kd.x
                                ,PID_PAR.rate_kp.x,PID_PAR.rate_ki.x,PID_PAR.rate_kd.x);

    control_data.roll = cascade_pid_roll_test(control_data.set_angel.y ,control_data.get_angel.y ,control_data.get_gyro.x
                                ,PID_PAR.angel_kp.x,PID_PAR.angel_ki.x,PID_PAR.angel_kd.x
                                ,PID_PAR.rate_kp.x,PID_PAR.rate_ki.x,PID_PAR.rate_kd.x);

//    control_data.pitch = cascade_pid_pitch_test(control_data.set_angel.x ,control_data.get_angel.x ,control_data.get_gyro.y
//                                ,2,0.05,0
//                                ,0.2,0,0);
//
//    control_data.roll = cascade_pid_roll_test(control_data.set_angel.y ,control_data.get_angel.y ,control_data.get_gyro.x
//                                ,2,0.05,0
//                                ,0.2,0,0);

 #ifdef OpenYawControl
    control_data.yaw = -cascade_pid_yaw_test(control_data.set_angel.z ,control_data.get_angel.z ,control_data.get_gyro.z
                                ,PID_PAR.angel_kp.z,PID_PAR.angel_ki.z,PID_PAR.angel_kd.z
                                ,PID_PAR.rate_kp.z,PID_PAR.rate_ki.z,PID_PAR.rate_kd.z);
 #endif
#endif
#ifdef HeightControl
    control_data.set_position.z = control_data.height_set;
//    out_disPID = pid_dis_z(control_data.set_position.z , 0,PID_PAR.dis_kp.z , PID_PAR.dis_ki.z ,PID_PAR.dis_kd.z);
    cas_PID_out = cas_height_pid_test(control_data.set_position.z ,control_data.get_position.z,control_data.get_vel.z
                             ,control_data.limit_vel_up.z,control_data.limit_vel_down.z
                             ,PID_PAR.dis_kp.z , PID_PAR.dis_ki.z ,PID_PAR.dis_kd.z
                             ,PID_PAR.vel_kp.z , PID_PAR.vel_ki.z ,PID_PAR.vel_kd.z);
    cas_PID_out = height_last_out + constrainf((cas_PID_out - height_last_out),-10,+10);
    height_last_out = cas_PID_out;
    control_data.height = cas_PID_out;
#endif
    //**********输出限幅**********
    control_data.pitch   = constrainf(control_data.pitch,min_pitch,max_pitch);
    control_data.roll    =  constrainf(control_data.roll,min_pitch,max_pitch);
    control_data.yaw     =  constrainf(control_data.yaw,min_pitch,max_pitch);
    control_data.height  =  constrainf(control_data.height,min_height,max_height);
    control_data.thrust=control_data.height+control_data.basethrust;
    control_data.thrust=constrainf(control_data.thrust,0,1000);
    //**********电机控制**********
    motorPWM.roll = control_data.roll;
    motorPWM.pitch = control_data.pitch;
    motorPWM.yaw = control_data.yaw;
    motorPWM.thrust = control_data.thrust;
    //**********电机控制**********
    if(control_data.motor_power)powerControl(&motorPWM,1);//电机转停控制
    else powerControl(&motorPWM,0);
}
void control_task(void *p)
{
    u32 t_now,t_preiod,t_pre ;
    void *delay;
    delay = delay_until_create("delay");  // 创建相对延时
    rt_thread_mdelay(500);
    control_data.set_angel.z = JY901_data.angel.z;
    while(1)
    {
#ifdef SetThrustRate
        //**********油门行程校准任务**********
        control_data.thrust = PPM_con.PPM_thrust ;
        motorPWM.roll = control_data.roll;
        motorPWM.pitch = control_data.pitch;
        motorPWM.yaw = control_data.yaw;
        motorPWM.thrust = control_data.thrust;
        if(control_data.motor_power)powerControl(&motorPWM,1);//电机转停控制
        else powerControl(&motorPWM,0);
#else
        //**********PID控制任务**********
         pid_control();
#endif
         //**********任务运行时间统计**********
         t_now = rt_tick_get();//要赋值给整型数据
         t_preiod=t_now-t_pre;
         task_time1.con_time = 1000/t_preiod;
         t_pre = t_now;

         delay_until_run(delay, 5);
    }
}

